/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.parsing.IInputOutput;

/**
 *
 * @author Jarrod Risley
 */
public class LogitechController extends GenericHID implements IInputOutput {

    // Stick Axes (Note: Mode button switches LeftX betwen Gamepad and Stick
    private static final int m_kAxisLeftX = 1;  // Left -
    private static final int m_kAxisLeftY = 2;  // Up -
    private static final int m_kAxisRightX = 4; // Left -
    private static final int m_kAxisRightY = 5; // Up -
    // Stick Buttons
    private static final int m_kButtonLeftTop = 9;
    private static final int m_kButtonRightTop = 10;
    // Gamepad Axes
    private static final int m_kAxisGameX = 6; // Left -
    private static final int m_kAxisGameY = 2; // Up -
    // Trigger Buttons (Note actual triggers LT, RT are acutally an Axis)
    private static final int m_kButtonLeftB = 5;
    private static final int m_kButtonRightB = 6;
    private static final int m_kAxisTriggers = 3; //Left Trigger +, Right Trigger -
    // Right Side Buttons
    private static final int m_kButtonA = 1;
    private static final int m_kButtonB = 2;
    private static final int m_kButtonX = 3;
    private static final int m_kButtonY = 4;
    //Central Buttons
    private static final int m_kButtonStart = 8;
    private static final int m_kButtonBack = 7;

    // Define Enumeration of Valid Axes
    public static class AxisType {

        public final int value;
        public static final LogitechController.AxisType kAxisLeftX = new LogitechController.AxisType(m_kAxisLeftX);
        public static final LogitechController.AxisType kAxisLeftY = new LogitechController.AxisType(m_kAxisLeftY);
        public static final LogitechController.AxisType kAxisRightX = new LogitechController.AxisType(m_kAxisRightX);
        public static final LogitechController.AxisType kAxisRightY = new LogitechController.AxisType(m_kAxisRightY);
        public static final LogitechController.AxisType kAxisGameX = new LogitechController.AxisType(m_kAxisGameX);
        public static final LogitechController.AxisType kAxisGameY = new LogitechController.AxisType(m_kAxisGameY);
        public static final LogitechController.AxisType kAxisTriggers = new LogitechController.AxisType(m_kAxisTriggers);

        private AxisType(int axisNumber) {
            this.value = axisNumber;
        }
    }

    // Define Enumeration of Valid Buttons
    public static class ButtonType {

        public final int value;
        public static final LogitechController.ButtonType kButtonLeftB = new LogitechController.ButtonType(m_kButtonLeftB);
        public static final LogitechController.ButtonType kButtonRightB = new LogitechController.ButtonType(m_kButtonRightB);
        public static final LogitechController.ButtonType kButtonLeftTop = new LogitechController.ButtonType(m_kButtonLeftTop);
        public static final LogitechController.ButtonType kButtonRightTop = new LogitechController.ButtonType(m_kButtonRightTop);
        public static final LogitechController.ButtonType kButtonA = new LogitechController.ButtonType(m_kButtonA);
        public static final LogitechController.ButtonType kButtonB = new LogitechController.ButtonType(m_kButtonB);
        public static final LogitechController.ButtonType kButtonX = new LogitechController.ButtonType(m_kButtonX);
        public static final LogitechController.ButtonType kButtonY = new LogitechController.ButtonType(m_kButtonY);
        public static final LogitechController.ButtonType kButtonStart = new LogitechController.ButtonType(m_kButtonStart);
        public static final LogitechController.ButtonType kButtonBack = new LogitechController.ButtonType(m_kButtonBack);

        private ButtonType(int buttonNumber) {
            this.value = buttonNumber;
        }
    }
    private DriverStation m_ds;
    private final int m_port;

    // Constructor
    public LogitechController(final int port) {
        m_ds = DriverStation.getInstance();
        m_port = port;
    }
       
    // GenericHID Interface Functions
    // Hand is ignored for all functions
    public double getRawAxis(int axisNum) {
        return m_ds.getStickAxis(m_port, axisNum);
    }
        
    public boolean getRawButton(int buttonNum) {
        return ((0x1 << (buttonNum - 1)) & m_ds.getStickButtons(m_port)) != 0;
    }

    public double getX(GenericHID.Hand hand) {
        return getRawAxis(LogitechController.AxisType.kAxisLeftX.value);
    }

    public double getY(GenericHID.Hand hand) {
        return getRawAxis(LogitechController.AxisType.kAxisLeftY.value);
    }

    public double getZ(GenericHID.Hand hand) {
        return getRawAxis(LogitechController.AxisType.kAxisRightX.value);
    }

    public double getTwist() {
        return getRawAxis(LogitechController.AxisType.kAxisRightY.value);
    }

    public double getThrottle() {
        return getRawAxis(LogitechController.AxisType.kAxisGameX.value);
    }

    public boolean getTrigger(GenericHID.Hand hand) {
        return getRawButton(LogitechController.ButtonType.kButtonLeftB.value);
    }

    public boolean getTop(GenericHID.Hand hand){
        return getRawButton(LogitechController.ButtonType.kButtonLeftTop.value);
    }

    public boolean getBumper(GenericHID.Hand hand){
        return getRawButton(LogitechController.ButtonType.kButtonA.value);
    }

    // Logitech Controller Specific Functions
    // Axes
    public double getLeftX () {
       return getRawAxis(LogitechController.AxisType.kAxisLeftX.value); 
    }
    public double getLeftY () {
       return getRawAxis(LogitechController.AxisType.kAxisLeftY.value); 
    }
    public double getRightX () {
       return getRawAxis(LogitechController.AxisType.kAxisRightX.value); 
    }
    public double getRightY () {
       return getRawAxis(LogitechController.AxisType.kAxisRightY.value); 
    }
    public double getGameX () {
       return getRawAxis(LogitechController.AxisType.kAxisGameX.value); 
    }
    public double getGameY () {
       return getRawAxis(LogitechController.AxisType.kAxisGameY.value); 
    }
    public double getTriggers () {
       return getRawAxis(LogitechController.AxisType.kAxisTriggers.value); 
    }
     
    // Buttons
    public boolean getButtonA () {
        return getRawButton(LogitechController.ButtonType.kButtonA.value);        
    }    
    public boolean getButtonB () {
        return getRawButton(LogitechController.ButtonType.kButtonB.value);        
    }    
    public boolean getButtonX () {
        return getRawButton(LogitechController.ButtonType.kButtonX.value);        
    }    
    public boolean getButtonY () {
        return getRawButton(LogitechController.ButtonType.kButtonY.value);        
    }    
    public boolean getButtonLB () {
        return getRawButton(LogitechController.ButtonType.kButtonLeftB .value);        
    }    
    public boolean getButtonRB () {
        return getRawButton(LogitechController.ButtonType.kButtonRightB.value);        
    }    
    public boolean getButtonLeftTop () {
        return getRawButton(LogitechController.ButtonType.kButtonLeftTop.value);        
    }    
    public boolean getButtonRightTop () {
        return getRawButton(LogitechController.ButtonType.kButtonRightTop.value);        
    }    
    public boolean getButtonStart () {
        return getRawButton(LogitechController.ButtonType.kButtonStart.value);        
    }    
    public boolean getButtonBack () {
        return getRawButton(LogitechController.ButtonType.kButtonBack.value);        
    }
    
    // Axes as buttons
    // A button axis is considered pressed if it reports more than .5 in the 
    //  direction of interest
    public boolean getButtonGameUp () {
        return (getRawAxis(LogitechController.AxisType.kAxisGameY.value) < -0.5);
    }
    public boolean getButtonGameDown () {
        return (getRawAxis(LogitechController.AxisType.kAxisGameY.value) > 0.5);
    }
    public boolean getButtonGameLeft () {
        return (getRawAxis(LogitechController.AxisType.kAxisGameX.value) < -0.5);
    }
    public boolean getButtonGameRight () {
        return (getRawAxis(LogitechController.AxisType.kAxisGameX.value) > 0.5);
    }
    public boolean getButtonLeftTrigger () {
        return (getRawAxis(LogitechController.AxisType.kAxisTriggers.value) > 0.5);
    }
    public boolean getButtonRightTrigger () {
        return (getRawAxis(LogitechController.AxisType.kAxisTriggers.value) < -0.5);
    }
            
    
}
